function [x, P] = TungDCM_KF1(x_1, P_1, z, r)

% x_1: (6x1) previous estimate value [C31;C32;C33;gx;gy;gz]
% P_1: (6x6) previous error covariance
% z  : (6x1) observer value [ax;ay;az;gx;gy;gz]
% x  : (6x1) estimation value at this time 
% P  : (6x6) error covariance

%% Init Kalman
T = 0.01;
q = 1;
a = 1; %0.4
ra0 = r;
rg0 = 1/a;
R = [ra0/a*eye(3,3), zeros(3,3);
      zeros(3,3),a*rg0*eye(3,3)];
H = eye(6,6);   % Note: a is divided by 'g' --> H = I;

%% Discrete model
C31 = x_1(1);
C32 = x_1(2);
C33 = x_1(3);
C3 = [0, -C33, C32; C33, 0, -C31;-C32, C31, 0];
Phik = [ eye(3) , C3 * T; zeros(3) , eye(3)]; % Phik = I + Phi(kT) * T
Qk = q*[T^3 * C3 *C3', T^2 * C3; T^2 * C3', T * eye(3)];

%% Time update
% State
x_ = Phik * x_1;
% Error covariance
P_ = Phik * P_1 * Phik' + Qk;    

%% Measurement update
% Compute Kalman gain
Kk = P_ * H' * inv(H * P_*H' + R);
% Update estimate with measurement z
x = x_ + Kk * (z - H * x_); 
% Normalization DCM(1:3)
x(1:3) = x(1:3)/norm(x(1:3));    
% Update the error covariance
P = (eye(6) - Kk * H) * P_;  

end

